/*
Circuito de control manual para fresadora CNC con interfaz paralelo.
 */

/*********************************************************
 * CONSTANTES
 *********************************************************/
const int sx = 13;                    /*Pulsos del eje X*/
const int dx = 12;                    /*Dirección del eje X*/
const int sy = 11;                    /*Pulsos del eje Y*/
const int dy = 10;                    /*Dirección del eje Y*/
const int sz =  9;                    /*Pulsos del eje Z*/
const int dz =  8;                    /*Dirección del eje Z*/
const int xIn = A0;                   /*JoyStick X*/
const int yIn = A1;                   /*JoyStick X*/
const int zIn = A2;                   /*JoyStick X*/
const int enX = 7;                    /*Habilitar el eje X*/
const int enY = 6;                    /*Habilitar el eje X*/
const int enZ = 5;                    /*Habilitar el eje X*/


//const long speedTable[] = 
//{
//  0, 
//  0, 
//  500000, 
//  250000, 
//  166667, 
//  100000, 
//  50000, 
//  28571, 
//  10000, 
//  3333, 
//  1667, 
//  833, 
//  417, 
//  200, 
//  200, 
//  200
//};

//const long speedTable[] = 
//{
//0	,
//0	,
//0	,
//0	,
//0	,
//0	,
//0	,
//125000	,
//62500	,
//41667	,
//31250	,
//25000	,
//20833	,
//17857	,
//15625	,
//13889	,
//12500	,
//4167	,
//2500	,
//1786	,
//1389	,
//1136	,
//962	,
//833	,
//735	,
//658	,
//595	,
//543	,
//500	,
//463	,
//431	,
//403	,
//379	,
//357	,
//338	,
//321	,
//305	,
//291	,
//278	,
//168	,
//121	,
//94	,
//77	,
//65	,
//57	,
//50	
//};

const long speedTable[] = 
{
0	,
0	,
0	,
125000	,
76923	,
40000	,
26667	,
20000	,
16000	,
13333	,
11429	,
10000	,
8889	,
8000	,
7273	,
6667	,
6154	,
5714	,
5333	,
5000	,
4706	,
4444	,
4211	,
4000	,
3636	,
3333	,
3077	,
2857	,
2667	,
2500	,
2353	,
2222	,
2105	,
2000	,
1778	,
1600	,
1455	,
1333	,
1231	,
1143	,
1067	,
1000	,
941	,
889	,
842	,
800	,

};


/*********************************************************
 * VARIABLES GLOBALES
 *********************************************************/
unsigned long currentTime;            /*Tiempo actual en uS*/
unsigned long xLastTime = 0;          /*Momento del intervalo anterior del pulso del eje X*/
unsigned long yLastTime = 0;          /*Momento del intervalo anterior del pulso del eje Y*/
unsigned long zLastTime = 0;          /*Momento del intervalo anterior del pulso del eje Z*/
int sxState = LOW;                    /*Estado: alto/bajo, de sx*/
int syState = LOW;                    /*Estado: alto/bajo, de sy*/
int szState = LOW;                    /*Estado: alto/bajo, de sz*/

long xInterval_ = 0;                    /*Periodo de los pulsos del eje X*/
long yInterval_ = 0;                    /*Periodo de los pulsos del eje Y*/
long zInterval_ = 0;                    /*Periodo de los pulsos del eje Z*/

unsigned long currentTimeMillis;      /*Tiempo actual en mS*/
unsigned long ctrlLastTime = 0;       /*Momento del intervalo anterior de la máquina del controlador*/
int xInValue;                         /*Valor leido con el ADC del eje X*/
int yInValue;                         /*Valor leido con el ADC del eje Y*/
int zInValue;                         /*Valor leido con el ADC del eje Z*/

int xCenter = 500;                    /*Valor del eje X del joystick en la posición central*/
int yCenter = 500;                    /*Valor del eje Y del joystick en la posición central*/
int zCenter = 500;                    /*Valor del eje Z del joystick en la posición central*/

int enXState = LOW;                    /*Estado de enX*/
int enYState = LOW;                    /*Estado de enY*/
int enZState = LOW;                    /*Estado de enZ*/

/*********************************************************
 * CONFIGURACIÓN DEL CONTROLADOR
 *********************************************************/
void setup() 
{
  pinMode(sx, OUTPUT);            
  pinMode(dx, OUTPUT);   
  pinMode(sy, OUTPUT);            
  pinMode(dy, OUTPUT);   
  pinMode(sz, OUTPUT);            
  pinMode(dz, OUTPUT);

  pinMode(enX, INPUT);
  pinMode(enY, INPUT);
  pinMode(enZ, INPUT);


  initCentralValues();

  /*Debug...*/
  Serial.begin(9600); 
}

/*********************************************************
 * FUNCIÓN PRINCIPAL
 *********************************************************/
void loop()
{
  currentTime = micros();
  currentTimeMillis = millis();
  xAxis(xInterval_);
  yAxis(yInterval_);
  zAxis(zInterval_);

  ctrlSys();
}

/*Control de velocidad del Eje X*/
void xAxis(long xInterval)
{
  if(xInterval) 
  {
    if(currentTime - xLastTime > xInterval) 
    {
      xLastTime = currentTime;   
      if (sxState == LOW)
      {
        sxState = HIGH;
      }
      else
      {
        sxState = LOW;
      }
      digitalWrite(sx, sxState);
    }
  }
}
/*Control de velocidad del Eje Y*/
void yAxis(long yInterval)
{
  if(yInterval)
  {
    if(currentTime - yLastTime > yInterval) 
    {
      yLastTime = currentTime;   
      if (syState == LOW)
      {
        syState = HIGH;
      }
      else
      {
        syState = LOW;
      }
      digitalWrite(sy, syState);
    }
  }
}
/*Control de velocidad del Eje Z*/
void zAxis(long zInterval)
{
  if(zInterval) 
  {
    if(currentTime - zLastTime > zInterval) 
    {
      zLastTime = currentTime;   
      if (szState == LOW)
      {
        szState = HIGH;
      }
      else
      {
        szState = LOW;
      }
      digitalWrite(sz, szState);
    }
  }
}

/*Sistema de control*/
void ctrlSys(void)
{
  if((currentTimeMillis - ctrlLastTime) > 274) 
  {/*Han pasado 100mS*/
    ctrlLastTime = currentTimeMillis;

    enXState = digitalRead(enX);
    enYState = digitalRead(enY);
//    enZState = digitalRead(enZ);

    xInValue = analogRead(xIn);  
    yInValue = analogRead(yIn);  
    zInValue = analogRead(zIn);    

    if(xInValue > xCenter)
    {
      digitalWrite(dx, LOW);
      xInValue -= xCenter;
    }
    else
    {
      digitalWrite(dx, HIGH);
      xInValue = xCenter - xInValue;
    }
    xInValue /= 10;
    if(xInValue > 45)
    {
      xInValue = 45;
    }
    if(enXState == HIGH)
    {
      xInterval_ = speedTable[xInValue];
    }
    else
    {
      xInterval_ = 0;
    }
    if(yInValue > yCenter)
    {
      digitalWrite(dy, HIGH);
      yInValue -= yCenter;
    }
    else
    {
      digitalWrite(dy, LOW);
      yInValue = yCenter - yInValue;
    }
    yInValue /= 10;
    if(yInValue > 45)
    {
      yInValue = 45;
    }    
    if(enYState == HIGH)
    {
      yInterval_ = speedTable[yInValue];
    }
    else
    {
      yInterval_ = 0;
    }
    if(zInValue > zCenter)
    {
      digitalWrite(dz, LOW);
      zInValue -= zCenter;
    }
    else
    {
      digitalWrite(dz, HIGH);
      zInValue = zCenter - zInValue;
    }
    zInValue /= 10;
    if(zInValue > 45)
    {
      zInValue = 45;
    }
//    if(enZState == HIGH)
//    {
      zInterval_ = speedTable[zInValue];
//    }
//    else
//    {
//      zInterval_ = 0;
//    }

 //   Serial.print(xInValue); 
 //   Serial.print(" "); 
 //   Serial.print(yInValue); 
 //   Serial.print(" "); 
  //  Serial.print(zInValue); 
  //  Serial.print(" ");     
  //  Serial.print(xInterval_); 
 //   Serial.print(" ");
 //   Serial.print(yInterval_); 
 //   Serial.print(" ");
//        Serial.print(zInterval_); 
 //   Serial.print(" ");
 //       Serial.print(dx); 
 //   Serial.print(" ");
 //       Serial.print(dy); 
  //  Serial.print(" ");


    // Serial.println(" "); 
  }
}

void initCentralValues(void)
{
  xInValue = analogRead(xIn);  
  yInValue = analogRead(yIn);  
  zInValue = analogRead(zIn); 
  xCenter = xInValue;
  yCenter = yInValue;
  zCenter = zInValue;
}















